#include "userConfig.h"
#include "lwip/netdb.h"
#include "lwip/mem.h"
#include <lwip/sockets.h>
#include "modbus.h"
#include "usart.h"
#include "task.h"

void tcp_modbus_server()
{
    int s = -1;
    modbus_t *ctx;
    modbus_mapping_t *mb_mapping;

    ctx = modbus_new_tcp("192.168.2.129", 1502);
    /* modbus_set_debug(ctx, TRUE); */

    mb_mapping = modbus_mapping_new_start_address(0, 0, 0, 0, 0x100, 70, 0, 0);
    if (mb_mapping == NULL) {
        usart_transmit("Failed to allocate the mapping: %s\n", modbus_strerror(errno));
        modbus_free(ctx);
        return ;
    }

    s = modbus_tcp_listen(ctx, 1);

    while (1) {
        modbus_tcp_accept(ctx, &s);
        for (;;) {
            uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
            int rc = 0;
            rc = modbus_receive(ctx, query);
            if (rc > 0) {
                /* rc is the query size */
                modbus_reply(ctx, query, rc, mb_mapping);
            } else if (rc == -1) {
                /* Connection closed by the client or error */
                break;
            }
        }
    }

    usart_transmit("Quit the loop: %s\n", modbus_strerror(errno));

    if (s != -1) {
        close(s);
    }
    modbus_mapping_free(mb_mapping);
    modbus_close(ctx);
    modbus_free(ctx);
}